#include <stdio.h>

#include <util/list.h>
#include <util/common_macros.h>
#include <util/msg.h>

#include "geometry.h"

#include "point.h"
#include "vertex.h"
#include "surface.h"
#include "triangle.h"
#include "delaunay.h"

#include "../matrix/matrix.h"

/* ========================================================================== */
//#define sqrt3	1.732050808F

/* ========================================================================== */
/*

static boolean __verify(ImpVertex * v1, ImpVertex * v2, ImpVertex * v3, ImpEdge *e1, ImpEdge *e2, ImpEdge *e3) {

	// ======================================================================
	if (v1 == NULL) {
		return FALSE;
	}
	if (v2 == NULL) {
		return FALSE;
	}
	if (v3 == NULL) {
		return FALSE;
	}
	if (e1 == NULL) {
		return FALSE;
	}
	if (e2 == NULL) {
		return FALSE;
	}
	if (e3 == NULL) {
		return FALSE;
	}
	// ======================================================================
	if (v1 == v2) {
		return FALSE;
	}
	if (v2 == v3) {
		return FALSE;
	}
	if (v3 == v1) {
		return FALSE;
	}

	if (e1 == e2) {
		return FALSE;
	}
	if (e2 == e3) {
		return FALSE;
	}
	if (e3 == e1) {
		return FALSE;
	}
	// ======================================================================
	if (!imp_segment_connect(IMP_SEGMENT(e1), v1, v2)) {
		return FALSE;
	}
	if (!imp_segment_connect(IMP_SEGMENT(e2), v2, v3)) {
		return FALSE;
	}
	if (!imp_segment_connect(IMP_SEGMENT(e3), v3, v1)) {
		return FALSE;
	}
	// ======================================================================

	return TRUE;
}
static boolean __ver(ImpTriangle * T1, ImpTriangle * T2, ImpEdge * es) {

	ImpVertex *v1, *v2, *v3;
	ImpEdge *e1, *e2, *e3;
	ImpVertex *vv1, *vv2, *vv3;
	ImpEdge *ee1, *ee2, *ee3;

	if (T1 == T2) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (!imp_triangle_have_edge(T1, es)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (!imp_triangle_have_edge(T2, es)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}

	if (!imp_triangle_is_ok(T1)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (!imp_triangle_is_ok(T2)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}

	imp_triangle_extract(T1, es, &v1, &v2, &v3, &e1, &e2, &e3);
	imp_triangle_extract(T2, es, &vv1, &vv2, &vv3, &ee1, &ee2, &ee3);

	if (v1 != vv2) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (v2 != vv1) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (v3 == vv3) {
		msg_warning("Ooops!\n");
		return FALSE;
	}

	if (!__verify(v1, v2, v3, e1, e2, e3)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}
	if (!__verify(vv1, vv2, vv3, ee1, ee2, ee3)) {
		msg_warning("Ooops!\n");
		return FALSE;
	}

}
 */


static void __flip_edge(ImpTriangle * T1, ImpTriangle * T2, ImpEdge * es) {

	ImpVertex *v1, *v2, *v3;
	ImpEdge *e1, *e2, *e3;

	ImpVertex *vv1, *vv2, *vv3;
	ImpEdge *ee1, *ee2, *ee3;

	imp_triangle_extract(T1, es, &v1, &v2, &v3, &e1, &e2, &e3);
	imp_triangle_extract(T2, es, &vv1, &vv2, &vv3, &ee1, &ee2, &ee3);

	/* Clean the triangles */
	imp_edge_remove_triangle(e1, T1);
	imp_edge_remove_triangle(e2, T1);
	imp_edge_remove_triangle(e3, T1);
	imp_edge_remove_triangle(ee1, T2);
	imp_edge_remove_triangle(ee2, T2);
	imp_edge_remove_triangle(ee3, T2);

	/* disconnect shared segment from v1 and v2 */
	imp_vertex_remove_segment(IMP_SEGMENT(es)->v[0], IMP_SEGMENT(es));
	imp_vertex_remove_segment(IMP_SEGMENT(es)->v[1], IMP_SEGMENT(es));

	/* flip the edge and connect v3 and v4 to es */
	imp_segment_set(IMP_SEGMENT(es), v3, vv3);

	/* set the modified triangles */
	imp_triangle_set(T1, es, e3, ee2);
	imp_triangle_set(T2, es, ee3, e2);

}
static void __legalize_edge(ImpVertex * v, ImpEdge * es, ImpTriangle * T) {

	ImpTriangle *T1, *T2;
	ImpVertex *vop;
	ImpIntersect inter;

	if (es->faces->size < 1 || es->faces->size > 2) {
		if (es->faces->size == 0) {
			msg_error("Edge not connect any face!");
		}
		if (es->faces->size > 2) {
			msg_error("An edge is shared by 3 or more faces!");
		}
	}

	if (es->faces->size == 1) {
		if (es->faces->first->data != T) {
			msg_error("Surface is corrupted!");
		}
		/* Edge doesn't have other Triangle. It is a border! */
		return;
	}


	/* Extract both triangles and opposite vertex of T2 */
	T1 = T;
	T2 = es->faces->first->data;
	if (T1 == T2) {
		T2 = es->faces->first->next->data;
	}
	vop = imp_triangle_get_opposite_vertex(T2, es);

	inter = imp_triangle_circle_intersect_point(T1, IMP_POINT(vop));

	switch (inter) {
		case IMP_INTERSECT_IN:

			/* edge flip */
			__flip_edge(T1, T2, es);

			ImpEdge * eop_t1;
			ImpEdge * eop_t2;

			/* select edges to legalize */
			eop_t1 = imp_triangle_get_opposite_edge(T1, v);
			eop_t2 = imp_triangle_get_opposite_edge(T2, v);

			/* legalize edges */
			__legalize_edge(v, eop_t1, T1);
			__legalize_edge(v, eop_t2, T2);

			break;
		case IMP_INTERSECT_ON:
		case IMP_INTERSECT_OUT:
		default:
			break;
	}
}
void imp_delaunay_init_mesh_square(ImpSurface * s, int x0, int y0, int x1, int y1) {
	ImpVertex *v1, *v2, *v3, *v4;
	ImpEdge *e1, *e2, *e3, *e4, *es;
	ImpTriangle *t1, *t2;

	x0--;
	y0--;
	x1++;
	y1++;

	v1 = imp_vertex_new((real) x0, (real) y0, .0);
	v2 = imp_vertex_new((real) x1, (real) y0, .0);
	v3 = imp_vertex_new((real) x1, (real) y1, .0);
	v4 = imp_vertex_new((real) x0, (real) y1, .0);

	e1 = imp_edge_new(v1, v2);
	e2 = imp_edge_new(v2, v3);
	e3 = imp_edge_new(v3, v4);
	e4 = imp_edge_new(v4, v1);
	es = imp_edge_new(v4, v2);

	t1 = imp_triangle_new(es, e4, e1);
	t2 = imp_triangle_new(es, e2, e3);

	imp_surface_add_vertex(s, v1);
	imp_surface_add_vertex(s, v2);
	imp_surface_add_vertex(s, v3);
	imp_surface_add_vertex(s, v4);

	imp_surface_add_edge(s, e1);
	imp_surface_add_edge(s, e2);
	imp_surface_add_edge(s, e3);
	imp_surface_add_edge(s, e4);
	imp_surface_add_edge(s, es);

	imp_surface_add_triangle(s, t1);
	imp_surface_add_triangle(s, t2);

}
boolean imp_delaunay_add_vertex_on_triangle(ImpSurface * surf, ImpVertex * v, ImpTriangle * T, ImpEdge * eon) {

	ImpTriangle *T1;
	ImpVertex *v1, *v2, *v3;
	ImpEdge *e1, *e2, *e3;

	ImpTriangle *T2;
	ImpVertex *vv1, *vv2, *vv3;
	ImpEdge *ee1, *ee2, *ee3;

	ImpEdge *ne1, *ne2, *ne3, *ne4;
	ImpTriangle *nt1, *nt2, *nt3, *nt4;

	switch (eon->faces->size) {
		case 1:
			/* ============================================================== */
			T1 = T;
			imp_triangle_extract(T1, eon, &v1, &v2, &v3, &e1, &e2, &e3);

			/* = Create new edges and triangles ============================= */
			ne1 = imp_edge_new(v, v1);
			ne2 = imp_edge_new(v, v2);
			ne3 = imp_edge_new(v, v3);
			nt1 = imp_triangle_new(ne1, ne3, e3);
			nt2 = imp_triangle_new(ne2, e2, ne3);

			/* = Store new edges and triangles ============================== */
			imp_surface_add_edge(surf, ne1);
			imp_surface_add_edge(surf, ne2);
			imp_surface_add_edge(surf, ne3);
			imp_surface_add_triangle(surf, nt1);
			imp_surface_add_triangle(surf, nt2);

			/* = Delete Triangle T ========================================== */
			imp_surface_remove_triangle(surf, T1);
			imp_object_delete(T1);

			/* = Delete Edge eon ============================================ */
			imp_surface_remove_edge(surf, eon);
			imp_object_delete(eon);

			/* = Legalize edges ============================================= */
			__legalize_edge(v, e3, nt1);
			__legalize_edge(v, e2, nt2);

			return TRUE;
			break;
		case 2:
			/* ============================================================== */

			T1 = T;
			T2 = eon->faces->first->data;
			if (T1 == T2) {
				T2 = eon->faces->first->next->data;
			}

			imp_triangle_extract(T1, eon, &v1, &v2, &v3, &e1, &e2, &e3);
			imp_triangle_extract(T2, eon, &vv1, &vv2, &vv3, &ee1, &ee2, &ee3);

			/* = Create new edges and triangles ============================= */
			ne1 = imp_edge_new(v, v1);
			ne2 = imp_edge_new(v, vv3);
			ne3 = imp_edge_new(v, vv1);
			ne4 = imp_edge_new(v, v3);

			nt1 = imp_triangle_new(ne1, ee2, ne2);
			nt2 = imp_triangle_new(ne2, ee3, ne3);
			nt3 = imp_triangle_new(ne3, e2, ne4);
			nt4 = imp_triangle_new(ne4, e3, ne1);

			/* = Store new edges and triangles ============================== */
			imp_surface_add_edge(surf, ne1);
			imp_surface_add_edge(surf, ne2);
			imp_surface_add_edge(surf, ne3);
			imp_surface_add_edge(surf, ne4);
			imp_surface_add_triangle(surf, nt1);
			imp_surface_add_triangle(surf, nt2);
			imp_surface_add_triangle(surf, nt3);
			imp_surface_add_triangle(surf, nt4);

			/* = Delete Triangle T1 ========================================= */
			imp_surface_remove_triangle(surf, T1);
			imp_object_delete(T1);

			/* = Delete Triangle T2 ========================================= */
			imp_surface_remove_triangle(surf, T2);
			imp_object_delete(T2);

			/* = Delete Edge eon ============================================ */
			imp_surface_remove_edge(surf, eon);
			imp_object_delete(eon);

			/* = Legalize edges ============================================= */
			__legalize_edge(v, ee2, nt1);
			__legalize_edge(v, ee3, nt2);
			__legalize_edge(v, e2, nt3);
			__legalize_edge(v, e3, nt4);

			return TRUE;
			break;
		default:
			msg_warning("Surface is not ok! Exist an edge with 3 or more faces!\n");
			return FALSE;
			break;
	}
	return TRUE;

}
boolean imp_delaunay_add_vertex_in_triangle(ImpSurface * s, ImpVertex * v, ImpTriangle * T) {

	ImpVertex * v1, *v2, *v3;
	ImpEdge *e1, *e2, *e3;
	ImpTriangle *t1, *t2, *t3;
	ImpEdge *ne1, *ne2, *ne3;

	imp_triangle_extract(T, NULL, &v1, &v2, &v3, &e1, &e2, &e3);

	/* = Removing the old triangle t ======================================== */
	imp_surface_remove_triangle(s, T);
	imp_object_delete(T);

	/* = Create the new three edges ========================================= */
	ne1 = imp_edge_new(v, v1);
	ne2 = imp_edge_new(v, v2);
	ne3 = imp_edge_new(v, v3);
	/* = Create the new three triangles ===================================== */
	t1 = imp_triangle_new(ne1, e1, ne2);
	t2 = imp_triangle_new(ne2, e2, ne3);
	t3 = imp_triangle_new(ne3, e3, ne1);

	/* = Add new edges and triangles at surface ============================== */
	imp_surface_add_edge(s, ne1);
	imp_surface_add_edge(s, ne2);
	imp_surface_add_edge(s, ne3);
	imp_surface_add_triangle(s, t1);
	imp_surface_add_triangle(s, t2);
	imp_surface_add_triangle(s, t3);

	/* = Legalize the new edges ============================================= */
	__legalize_edge(v, e1, t1);
	__legalize_edge(v, e2, t2);
	__legalize_edge(v, e3, t3);

	return TRUE;
}
boolean imp_delaunay_add_vertex(ImpSurface * surf, ImpVertex * v) {

	ListIterator * it;
	ImpTriangle * T;
	ImpEdge * eon;
	ImpIntersect inter;

	/* get the triangle that intersects with v */
	it = surf->faces->first;
	while (it) {
		T = IMP_TRIANGLE(it->data);
		it = it->next;
		inter = imp_triangle_intersect_point(T, IMP_POINT(v), &eon);
		if (IMP_INTERSECT_OUT == inter) continue;
		break;
	}

	switch (inter) {
		case IMP_INTERSECT_IN:
			return imp_delaunay_add_vertex_in_triangle(surf, v, T);
			break;
		case IMP_INTERSECT_ON:
			return imp_delaunay_add_vertex_on_triangle(surf, v, T, eon);
			break;
		case IMP_INTERSECT_OUT:
		default:
			return FALSE;
			break;
	}
	return FALSE;
}
ImpSurface * imp_delaunay_from_matrix(ImpMatrix * mat, boolean remove_anchors) {

	ImpSurface * surf = NULL;
	ImpVertex * v1 = NULL;
	ImpVertex * v2 = NULL;
	ImpVertex * v3 = NULL;
	ImpVertex * v4 = NULL;

	int r, c, _r, i;
	int rows, columns;

	_return_val_if_fail(mat, NULL);

	surf = imp_surface_new();
	imp_delaunay_init_mesh_square(surf, -1, -1, mat->columns, mat->rows);
	v1 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 0));
	v2 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 1));
	v3 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 2));
	v4 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 3));

	rows = mat->rows;
	columns = mat->columns;
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			if (mat->data[i] == 0) continue;

			ImpVertex * v;
			v = imp_vertex_new((real) c, (real) r, 0.0);

			if (imp_delaunay_add_vertex(surf, v)) {
				imp_surface_add_vertex(surf, v);
			} else {
				imp_object_delete(v);
			}
		}
	}

	if (remove_anchors) {
		imp_surface_remove_vertex(surf, v1);
		imp_surface_remove_vertex(surf, v2);
		imp_surface_remove_vertex(surf, v3);
		imp_surface_remove_vertex(surf, v4);

		imp_object_delete(v1);
		imp_object_delete(v2);
		imp_object_delete(v3);
		imp_object_delete(v4);

	}

	return surf;
}
ImpSurface * imp_delaunay_from_points(ImpMatrix * Mat, int rows, int columns) {

	ImpSurface * surf = NULL;
	ImpVertex * v1 = NULL;
	ImpVertex * v2 = NULL;
	ImpVertex * v3 = NULL;
	ImpVertex * v4 = NULL;

	_return_val_if_fail(Mat, NULL);

	surf = imp_surface_new();
	
	imp_delaunay_init_mesh_square(surf, -1, -1, columns, rows);
	v1 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 0));
	v2 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 1));
	v3 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 2));
	v4 = IMP_VERTEX(list_get_data_at_index(surf->vertices, 3));

	int i;
	real x, y;
	ImpVertex * v;

	for (i = 0; i < Mat->size; i += 2) {

		x = Mat->data[i];
		y = Mat->data[i + 1];

		v = imp_vertex_new(x, y, 0.0);

		if (imp_delaunay_add_vertex(surf, v)) {
			imp_surface_add_vertex(surf, v);
		} else {
			imp_object_delete(v);
		}

	}
	imp_surface_remove_vertex(surf, v1);
	imp_surface_remove_vertex(surf, v2);
	imp_surface_remove_vertex(surf, v3);
	imp_surface_remove_vertex(surf, v4);

	imp_object_delete(v1);
	imp_object_delete(v2);
	imp_object_delete(v3);
	imp_object_delete(v4);

	return surf;
}